/* 简介：cothread 是一个轻量级协程调度器，由纯C语言实现，易于移植到各种单片机。
 * 同时，由于该调度器仅仅运行在一个实际线程中，所以它也适用于服务器高并发场景。
 *
 * 版本: 1.0.0   2019/02/25
 *
 * 作者: 覃攀 <qinpan1003@qq.com>
 *
 */

#include "rtos.h"

static unsigned int current_tick;
static struct cothread_scheduler *scheduler_list[20];
static int scheduler_nr = 0;

static int scheduler_init(struct cothread_scheduler *scheduler)
{
    int i = 0;
    ccb_t *ccb = NULL;

    if (scheduler->cothread_inited)
    {
        LOG_ERR("cothread_init:reinit\n");
        return -1;
    }
    
    for (i = 0; i < scheduler->ccb_nr; i++)
    {
        ccb = scheduler->ccb_table + i;

        if (i != (scheduler->ccb_nr - 1))
            ccb->next = scheduler->ccb_table + i + 1;
        
        if (i == 0)
            scheduler->free_ccb_list = ccb;
        else
            ccb->prev = scheduler->ccb_table + i - 1;

        scheduler->cothread_runtime_state.free_nr++;
    }
    
    scheduler->cothread_inited = 1;
    return 0;
}

ccb_t *cothread_create_on_scheduler(struct cothread_scheduler *scheduler, 
        coresult_t (*fun)(ccb_t *ccb), void *arg, cothread_prio_t prio)
{
    irq_state_t irq_stat;
    ccb_t *ccb = NULL;

    /* 从 free 队列头取一个节点 */
    if (scheduler->free_ccb_list == NULL)
    {
        LOG("[war] ccb lack %p %p, %d.\n", scheduler, fun, prio);
        return NULL;
    }

    cothread_enter_critical();

    ccb = scheduler->free_ccb_list;
    if (ccb->status != COTHREAD_STATUS_NONE)
    {
        LOG("[bug] ccb status invalid %p %p, %d.\n", scheduler, fun, prio);
        cothread_exit_critical();
        return NULL;
    }

    if (fun == NULL)
    {
        LOG("[bug] new fun is NULL.\n");
        cothread_exit_critical();
        return NULL;
    }

    scheduler->free_ccb_list = ccb->next;
    scheduler->cothread_runtime_state.free_nr--;

    ccb->prio = prio;
    ccb->fun = fun;
    ccb->arg = arg;
    ccb->status = COTHREAD_STATUS_READY;

    /* 节点插入    ready 队列 */
    ccb->prev = NULL;
    ccb->next = scheduler->ready_ccb_list[ccb->prio];

    if (scheduler->ready_ccb_list[ccb->prio] != NULL)
        scheduler->ready_ccb_list[ccb->prio]->prev = ccb;
    scheduler->ready_ccb_list[ccb->prio] = ccb;

    ccb->scheduler = scheduler;

    scheduler->cothread_runtime_state.ready_nr[ccb->prio]++;

    cothread_exit_critical();
    return ccb;
}

ccb_t *cothread_create(coresult_t (*fun)(ccb_t *ccb), 
                void *arg, cothread_prio_t prio)
{
    return cothread_create_on_scheduler(scheduler_list[0], fun, arg, prio);
}

/* 注意：线程只能删除自己，不能删除其他线程，否则调度器遍历会出错 */
int cothread_delete(ccb_t *ccb)
{
    irq_state_t irq_stat;
    struct cothread_scheduler *scheduler = ccb->scheduler;

    if (ccb->status != COTHREAD_STATUS_READY)
    {
        LOG("[bug] delete thread status invalid %p, %d.\n", ccb->fun, ccb->status);
        return -1;
    }

    cothread_enter_critical();

    /* 从队列中移除 */
    if (ccb == scheduler->ready_ccb_list[ccb->prio])
        scheduler->ready_ccb_list[ccb->prio] = ccb->next;
    else
        ccb->prev->next = ccb->next;

    if (ccb->next != NULL)
        ccb->next->prev = ccb->prev;

    scheduler->cothread_runtime_state.ready_nr[ccb->prio]--;

    memset(ccb, 0, sizeof(*ccb));

    /* 放入   free 队列 */
    ccb->prev = NULL;
    ccb->next = scheduler->free_ccb_list;

    if (scheduler->free_ccb_list != NULL)
        scheduler->free_ccb_list->prev = ccb;
    scheduler->free_ccb_list = ccb;

    scheduler->cothread_runtime_state.free_nr++;

    cothread_exit_critical();

    return 0;
}

void cothread_yeild(ccb_t *ccb)
{
    if (ccb->status != COTHREAD_STATUS_READY)
        LOG("[bug] yeild thread status invalid %p, %d.\n", ccb->fun, ccb->status);
}

int cothread_sleep(ccb_t *ccb, unsigned int tick)
{
    irq_state_t irq_stat;
    ccb_t *ccb_tmp;
    struct cothread_scheduler *scheduler = ccb->scheduler;

    if (ccb->status != COTHREAD_STATUS_READY)
    {
        LOG("[bug] sleep thread status invalid %p, %d.\n", ccb->fun, ccb->status);
        return -1;
    }

    cothread_enter_critical();

    /* 从 ready 队列中移除  */
    if (ccb == scheduler->ready_ccb_list[ccb->prio])
        scheduler->ready_ccb_list[ccb->prio] = ccb->next;
    else
        ccb->prev->next = ccb->next;
    
    if (ccb->next != NULL)
        ccb->next->prev = ccb->prev;
    
    ccb->status = COTHREAD_STATUS_DELAY;
    ccb->timeout_tick = current_tick + tick;
    scheduler->cothread_runtime_state.ready_nr[ccb->prio]--;

    /* 放入 delay   队列， delay 队列在放入的时候进行排序 */
    scheduler->cothread_runtime_state.delay_nr++;
    ccb->prev = NULL;
    ccb->next = NULL;

    /* delay 队列为空，直接放入链表头 */
    if (scheduler->delay_ccb_list == NULL)
    {
        scheduler->delay_ccb_list = ccb;
        cothread_exit_critical();
        return 0;
    }

    /* delay 队列非空，升序插入链表 */
    ccb_tmp = scheduler->delay_ccb_list;
    while (ccb_tmp->next != NULL && ccb_tmp->timeout_tick < ccb->timeout_tick)
        ccb_tmp = ccb_tmp->next;

    if (ccb_tmp->timeout_tick < ccb->timeout_tick)
    {
        ccb_tmp->next = ccb;
        ccb->prev = ccb_tmp;
    }
    else
    {
        ccb->next = ccb_tmp;
        ccb->prev = ccb_tmp->prev;
        
        ccb_tmp->prev = ccb;
        if (ccb_tmp == scheduler->delay_ccb_list)
            scheduler->delay_ccb_list = ccb;
        else
            ccb->prev->next = ccb;
    }

    cothread_exit_critical();
    return 0;
}

/* 线程唤醒后自己通过 ccb->event_wakeup 判断是否超时 */
int cothread_wait(ccb_t *ccb, unsigned int event_mask, unsigned int tick)
{
    int ret;
    irq_state_t irq_stat;
    struct cothread_scheduler *scheduler = ccb->scheduler;
    
    if (ccb->status != COTHREAD_STATUS_READY)
    {
        LOG("[bug] wait thread status invalid %p, %d.\n", ccb->fun, ccb->status);
        return -1;
    }

    ccb->event_mask = event_mask;

    cothread_enter_critical();

    /* 信号已到达，直接返回 */
    if (ccb->event_mask & ccb->event_signaled)
    {
        ccb->event_wakeup = 1;
        cothread_exit_critical();
        return 0;
    }

    if (tick != 0)
    {
        /* 定时等待，放入 delay 队列 */
        ret = cothread_sleep(ccb, tick);
        cothread_exit_critical();
        return ret;
    }
    else
    {
        /* 永久等待，放入   wait 队列，便于统计 */
        /* 从 ready 队列中移除 */
        if (ccb == scheduler->ready_ccb_list[ccb->prio])
            scheduler->ready_ccb_list[ccb->prio] = ccb->next;
        else
            ccb->prev->next = ccb->next;
        
        if (ccb->next != NULL)
            ccb->next->prev = ccb->prev;

        scheduler->cothread_runtime_state.ready_nr[ccb->prio]--;

        ccb->status = COTHREAD_STATUS_WAIT;

        /* 放入   wait 队列 */
        ccb->prev = NULL;
        ccb->next = scheduler->wait_ccb_list;

        if (scheduler->wait_ccb_list != NULL)
            scheduler->wait_ccb_list->prev = ccb;
        scheduler->wait_ccb_list = ccb;

        scheduler->cothread_runtime_state.wait_nr++;

        cothread_exit_critical();
        return 0;
    }
}

/* 注意：只能在线程间发送信号，不能从中断往线程发送信号，因为没有互斥机制 */
/* 中断可以通过软中断机制向线程传递信息，软中断运行在调度器中，不需要和线程互斥 */
int cothread_signal(ccb_t *ccb, unsigned int event_mask)
{
    irq_state_t irq_stat;
    struct cothread_scheduler *scheduler = ccb->scheduler;

    cothread_enter_critical();
    ccb->event_signaled |= event_mask;

    if ((ccb->event_mask & ccb->event_signaled) == 0
        || (ccb->status != COTHREAD_STATUS_DELAY && ccb->status != COTHREAD_STATUS_WAIT))
    {
        cothread_exit_critical();
        return 0;
    }

    /* 从 delay 或者 wait 队列中移除 */
    if (ccb == scheduler->delay_ccb_list)
        scheduler->delay_ccb_list = ccb->next;
    else if (ccb == scheduler->wait_ccb_list)
        scheduler->wait_ccb_list = ccb->next;
    else
        ccb->prev->next = ccb->next;
    
    if (ccb->next != NULL)
        ccb->next->prev = ccb->prev;

    if (ccb->status == COTHREAD_STATUS_DELAY)
        scheduler->cothread_runtime_state.delay_nr--;
    else
        scheduler->cothread_runtime_state.wait_nr--;

    ccb->event_wakeup = 1;
    ccb->status = COTHREAD_STATUS_READY;
    
    /* 放入 ready 队列 */
    ccb->prev = NULL;
    ccb->next = scheduler->ready_ccb_list[ccb->prio];

    if (scheduler->ready_ccb_list[ccb->prio] != NULL)
        scheduler->ready_ccb_list[ccb->prio]->prev = ccb;
    scheduler->ready_ccb_list[ccb->prio] = ccb;

    scheduler->cothread_runtime_state.ready_nr[ccb->prio]++;

    cothread_exit_critical();
    return 0;
}

static void cothread_handle_ready_quene_prio(ccb_t *ccb)
{
    coresult_t ret;
    ccb_t *ccb_tmp = NULL;
    unsigned int tick_tmp;
    struct cothread_scheduler *scheduler = ccb->scheduler;

    while (ccb)
    {
        if (ccb->status != COTHREAD_STATUS_READY)
        {
            LOG("[bug] ready thread status invalid %p, %d\n", ccb->fun, ccb->status);
            return;
        }

        if (ccb->fun == NULL)
        {
            LOG("[bug] ready thread fun is NULL\n");
            return;
        }

        ccb_tmp = ccb;
        ccb = ccb->next;

        scheduler->current_thread = ccb_tmp;
        tick_tmp = current_tick;
        ret = ccb_tmp->fun(ccb_tmp);
        tick_tmp = current_tick - tick_tmp;
        scheduler->current_thread = NULL;

        if (tick_tmp > (OS_HZ / 10))
            LOG("[warn][%p][%u] sched too slow.\n", ccb_tmp->fun, tick_tmp);

        if (tick_tmp > scheduler->cothread_runtime_state.sched_tick_max)
        {
            scheduler->cothread_runtime_state.sched_tick_max = tick_tmp;
            scheduler->cothread_runtime_state.sched_tick_max_fun = ccb_tmp->fun;
        }

        if (ret == STATUS_DONE)
            cothread_delete(ccb_tmp);

        scheduler->cothread_runtime_state.sched_nr++;
    }
}

static int cothread_handle_ready_quene(struct cothread_scheduler *scheduler)
{
    int i;
    
    for (i = 0; i < scheduler->prio_nr; i++)
    {
        if (scheduler->ready_ccb_list[i] != NULL)
        {
            cothread_handle_ready_quene_prio(scheduler->ready_ccb_list[i]);
            return 1;
        }
    }

    return 0;
}

static int cothread_handle_delay_quene(struct cothread_scheduler *scheduler)
{
    int ret = 0;
    ccb_t *ccb;
    irq_state_t irq_stat;
    
    cothread_enter_critical();
    ccb = scheduler->delay_ccb_list;

    while (ccb)
    {
        /* 类似 time_before */
        if (time_before(current_tick, ccb->timeout_tick))
            break;

        ret++;
        
        /* 从 delay 队列中移除 */
        scheduler->delay_ccb_list = ccb->next;
        
        if (ccb->next != NULL)
            ccb->next->prev = ccb->prev;
        scheduler->cothread_runtime_state.delay_nr--;

        ccb->event_wakeup = 0;
        ccb->status = COTHREAD_STATUS_READY;
        
        /* 放入 ready 队列 */
        ccb->prev = NULL;
        ccb->next = scheduler->ready_ccb_list[ccb->prio];

        if (scheduler->ready_ccb_list[ccb->prio] != NULL)
            scheduler->ready_ccb_list[ccb->prio]->prev = ccb;
        scheduler->ready_ccb_list[ccb->prio] = ccb;
        scheduler->cothread_runtime_state.ready_nr[ccb->prio]++;

        ccb = scheduler->delay_ccb_list;
    }

    cothread_exit_critical();

    return ret;
}

int cothread_loop_once(struct cothread_scheduler *scheduler)
{
    int ret = 0;
    static unsigned int last_loop_tick;

    ret = cothread_handle_ready_quene(scheduler);
    
    if (last_loop_tick == current_tick)
        return ret;
    
    ret += cothread_handle_delay_quene(scheduler);
    last_loop_tick = current_tick;

    return ret;
}

void cothread_start(void)
{
    int i;
    struct cothread_scheduler *scheduler;
        
    system_timer_start();

    while (1)
    {
        for (i = 0; i < scheduler_nr; i++)
        {
            scheduler = scheduler_list[i];
            cothread_loop_once(scheduler);
        }
    }
}

void system_tick(void)
{    
    current_tick++;
    cothread_scheduler_wakeup();
}

unsigned int get_system_tick(void)
{
    return current_tick;
}

void cothread_show_stat(void)
{
    int i, j;
    struct cothread_scheduler *scheduler;

    for (i = 0; i < scheduler_nr; i++)
    {
        scheduler = scheduler_list[i];
    
        struct cothread_stat stat = scheduler->cothread_runtime_state;
        
        LOG("os show[%d][%p]:\n\n"
                    "  tick:%u\n"
                    "  sched:%lu\n"
                    "  slow:%p,%u\n"
                    "\n"
                    "  free_quene:%d/%d\n"
                    "  delay_quene:%d\n"
                    "  wait_quene:%d\n",
                    i, scheduler, current_tick, 
                    stat.sched_nr,
                    stat.sched_tick_max_fun, stat.sched_tick_max,
                    stat.free_nr, scheduler->ccb_nr,
                    stat.delay_nr,
                    stat.wait_nr);

        for (j = 0; j < scheduler->prio_nr; j++)
            LOG("  ready_quene[%d]:%d\n", j, stat.ready_nr[j]);

        LOG("\n");
    }
}

struct cothread_scheduler *alloc_scheduler(int ccb_nr, int prio_nr, void *priv)
{
    ccb_t *ccb_table = malloc(ccb_nr * sizeof(ccb_t));
    if (ccb_table == NULL)
    {
        LOG_ERR("alloc_scheduler malloc ccb failed.\n");
        return NULL;
    }
    memset(ccb_table, 0, ccb_nr * sizeof(ccb_t));

    ccb_t **ready_ccb_list = malloc(prio_nr * sizeof(ccb_t *));
    if (ready_ccb_list == NULL)
    {
        LOG_ERR("alloc_scheduler malloc ready list failed.\n");
        free(ccb_table);
        return NULL;
    }
    memset(ready_ccb_list, 0, prio_nr * sizeof(ccb_t *));

    struct cothread_scheduler *scheduler = malloc(sizeof(struct cothread_scheduler));
    if (scheduler == NULL)
    {
        LOG_ERR("alloc_scheduler malloc scheduler failed.\n");
        free(ccb_table);
        free(ready_ccb_list);
        return NULL;
    }
    memset(scheduler, 0, sizeof(struct cothread_scheduler));

    scheduler->ccb_table = ccb_table;
    scheduler->ccb_nr = ccb_nr;
    scheduler->prio_nr = prio_nr;
    scheduler->ready_ccb_list = ready_ccb_list;

    scheduler_init(scheduler);

    scheduler->priv = priv;
    scheduler_list[scheduler_nr++] = scheduler;

    return scheduler;
}

int cothread_init(void)
{
    /* 建立默认调度器0 */
    alloc_scheduler(COTHREAD_NR, THREAD_PRIO_NR, NULL);

    return 0;
}

struct cothread_scheduler *default_scheduler(void)
{
    return scheduler_list[0];
}

